#include "system.hpp"

PFIC_Register_t *PFIC_register = (PFIC_Register_t*)0xe000e000;
SysTick_Register_t *SysTick_register = (SysTick_Register_t*)0xe000f000;

TIM_Register_t *TIM2_register = (TIM_Register_t*)0x40000000;
TIM_Register_t *TIM3_register = (TIM_Register_t*)0x40000400;
TIM_Register_t *TIM4_register = (TIM_Register_t*)0x40000800;
TIM_Register_t *TIM5_register = (TIM_Register_t*)0x40000c00;
TIM_Register_t *TIM6_register = (TIM_Register_t*)0x40001000;
TIM_Register_t *TIM7_register = (TIM_Register_t*)0x40001400;
USART_Register_t *UART6_register = (USART_Register_t*)0x40001800;
USART_Register_t *UART7_register = (USART_Register_t*)0x40001c00;
USART_Register_t *UART8_register = (USART_Register_t*)0x40002000;
RTC_Register_t *RTC_register = (RTC_Register_t*)0x40002800;
WWDG_Register_t *WWDG_register = (WWDG_Register_t*)0x40002c00;
IWDG_Register_t *IWDG_register = (IWDG_Register_t*)0x40003000;
SPI_Register_t *SPI2_register = (SPI_Register_t*)0x40003800;
SPI_Register_t *SPI3_register = (SPI_Register_t*)0x40003c00;
USART_Register_t *USART2_register = (USART_Register_t*)0x40004400;
USART_Register_t *USART3_register = (USART_Register_t*)0x40004800;
USART_Register_t *UART4_register = (USART_Register_t*)0x40004c00;
USART_Register_t *UART5_register = (USART_Register_t*)0x40005000;
I2C_Register_t *I2C1_register = (I2C_Register_t*)0x40005400;
I2C_Register_t *I2C2_register = (I2C_Register_t*)0x40005800;


BKP_Register_t *BKP_register = (BKP_Register_t*)0x40006c04;
PWR_Register_t *PWR_register = (PWR_Register_t*)0x40007000;
DAC_Register_t *DAC_register = (DAC_Register_t*)0x40007400;
AFIO_Register_t *AFIO_register = (AFIO_Register_t*)0x40010000;
EXTI_Register_t *EXTI_register = (EXTI_Register_t*)0x40010400;
GPIO_Register_t *GPIOA_register = (GPIO_Register_t*)0x40010800;
GPIO_Register_t *GPIOB_register = (GPIO_Register_t*)0x40010c00;
GPIO_Register_t *GPIOC_register = (GPIO_Register_t*)0x40011000;
GPIO_Register_t *GPIOD_register = (GPIO_Register_t*)0x40011400;
GPIO_Register_t *GPIOE_register = (GPIO_Register_t*)0x40011800;
ADC_Register_t *ADC1_register = (ADC_Register_t*)0x40012400;
ADC_Register_t *ADC2_register = (ADC_Register_t*)0x40012800;
TIM_Register_t *TIM1_register = (TIM_Register_t*)0x40012c00;
SPI_Register_t *SPI1_register = (SPI_Register_t*)0x40013000;
TIM_Register_t *TIM8_register = (TIM_Register_t*)0x40013400;
USART_Register_t *USART1_register = (USART_Register_t*)0x40013800;
TIM_Register_t *TIM9_register = (TIM_Register_t*)0x40014c00;
TIM_Register_t *TIM10_register = (TIM_Register_t*)0x40015000;
SDIO_Register_t *SDIO_register = (SDIO_Register_t*)0x40018000;
DMA_Register_t *DMA1_register = (DMA_Register_t*)0x40020000;
DMA_Channel_Register_t *DMA1_Channel1_register = (DMA_Channel_Register_t*)0x40020008;
DMA_Channel_Register_t *DMA1_Channel2_register = (DMA_Channel_Register_t*)0x4002001c;
DMA_Channel_Register_t *DMA1_Channel3_register = (DMA_Channel_Register_t*)0x40020030;
DMA_Channel_Register_t *DMA1_Channel4_register = (DMA_Channel_Register_t*)0x40020044;
DMA_Channel_Register_t *DMA1_Channel5_register = (DMA_Channel_Register_t*)0x40020058;
DMA_Channel_Register_t *DMA1_Channel6_register = (DMA_Channel_Register_t*)0x4002006c;
DMA_Channel_Register_t *DMA1_Channel7_register = (DMA_Channel_Register_t*)0x40020080;
DMA_Channel_Register_t *DMA1_Channel8_register = (DMA_Channel_Register_t*)0x40020094;
DMA_Register_t *DMA2_register = (DMA_Register_t*)0x40020400;
DMA_Channel_Register_t *DMA2_Channel1_register = (DMA_Channel_Register_t*)0x40020408;
DMA_Channel_Register_t *DMA2_Channel2_register = (DMA_Channel_Register_t*)0x4002041c;
DMA_Channel_Register_t *DMA2_Channel3_register = (DMA_Channel_Register_t*)0x40020430;
DMA_Channel_Register_t *DMA2_Channel4_register = (DMA_Channel_Register_t*)0x40020444;
DMA_Channel_Register_t *DMA2_Channel5_register = (DMA_Channel_Register_t*)0x40020458;
DMA_Channel_Register_t *DMA2_Channel6_register = (DMA_Channel_Register_t*)0x4002046c;
DMA_Channel_Register_t *DMA2_Channel7_register = (DMA_Channel_Register_t*)0x40020480;
DMA_Channel_Register_t *DMA2_Channel8_register = (DMA_Channel_Register_t*)0x40020490;
DMA_Channel_Register_t *DMA2_Channel9_register = (DMA_Channel_Register_t*)0x400204a0;
DMA_Channel_Register_t *DMA2_Channel10_register = (DMA_Channel_Register_t*)0x400204b0;
DMA_Channel_Register_t *DMA2_Channel11_register = (DMA_Channel_Register_t*)0x400204c0;
DMA_Register_t *DMA2_EXTEM_register = (DMA_Register_t*)0x400204d0;

RCC_Register_t *RCC_register = (RCC_Register_t*)0x40021000;
EXTEN_Register_t *EXTEN_register = (EXTEN_Register_t*)0x40023800;
USBFSD_Register_t *USBFSD_register = (USBFSD_Register_t*)0x50000000;
USBFSH_Register_t *USBFSH_register = (USBFSH_Register_t*)0x50000000;
USBHSD_Register_t *USBHSD_register = (USBHSD_Register_t*)0x40023400;
USBHSH_Register_t *USBHSH_register = (USBHSH_Register_t*)0x40023400;

CAN_Register_t *CAN1_register = (CAN_Register_t*)0x40006400;
CAN_Register_t *CAN2_register = (CAN_Register_t*)0x40006800;

ETH_Register_t *ETH_register = (ETH_Register_t*)0x40028000;

static unsigned int __system_clock_bus_frequency[3] = {0};
volatile long unsigned int system_tick = 0;

uint32_t p_us = 18;
uint32_t p_ms = 18000;

static void System_clockInit(){
    /* Set clock to 144M HSE. */
    volatile uint32_t startup_counter = 0, hse_status = 0;

    RCC_register->CTLR |= ((uint32_t)0x01 << 16);

    /* Wait till HSE is ready and if Time out is reached exit */
    do{
        hse_status = RCC_register->CTLR & (0x01 << 17);
        startup_counter++;
    } while ( (hse_status == 0) && (startup_counter != 0x4fff) );

    if ( (RCC_register->CTLR & (0x01 << 17)) != 0 ){
        hse_status = (uint32_t)0x01;
    }else{
        hse_status = (uint32_t)0x00;
    }

    if ( hse_status == (uint32_t)0x01 ){
        /* HCLK = SYSCLK */
        RCC_register->CFGR0 |= (uint32_t)0x00000000;
        /* PCLK2 = HCLK */
        RCC_register->CFGR0 |= (uint32_t)0x00000000;
        /* PCLK1 = HCLK / 2 */
        RCC_register->CFGR0 |= (uint32_t)(0b100 << 8);

        /*  PLL configuration: PLLCLK = HSE * 18 = 144 MHz */
        RCC_register->CFGR0 &= (uint32_t)((uint32_t)~((0x01 << 16) | (0x01 << 17) | (0x0f << 18)));
        RCC_register->CFGR0 |= (uint32_t)((0x01 << 16));
        
        /* Enable PLL */
        RCC_register->CTLR |= (0x01 << 24);
        /* Wait till PLL is ready */
        while((RCC_register->CTLR & (0x01 << 25)) == 0);
        /* Select PLL as system clock source */
        RCC_register->CFGR0 &= (uint32_t)((uint32_t)~(0x03));
        RCC_register->CFGR0 |= (uint32_t)(0x02);
        /* Wait till PLL is used as system clock source */
        while ((RCC_register->CFGR0 & (uint32_t)(0x03 << 2)) != (uint32_t)0x08);
        __system_clock_bus_frequency[(int)(System::ClockBus::AHB)] = 144 * 1000 * 1000;
        __system_clock_bus_frequency[(int)(System::ClockBus::APB1)] = 72 * 1000 * 1000;
        __system_clock_bus_frequency[(int)(System::ClockBus::APB2)] = 144 * 1000 * 1000;
    }else{
        /*
            * If HSE fails to start-up, the application will have wrong clock
        * configuration. User can add here some code to deal with this error
            */
    }
    return;
}

static void System_delayInit(){
    system_tick = 0;            /* Clear system tick. */
    SysTick_register->SR &= ~(0x01 << 0);
    SysTick_register->CMP = 17999;
    SysTick_register->CNT = 0;
    SysTick_register->CTLR = 0xb;
    System::setInterruptChannelStatus(System::InterruptChannel::SysTick,true); /* Enable SysTick interrupt. */
    return;
}

extern "C" void System_init(void){
    RCC_register->CTLR |= (uint32_t)0x00000001;
    RCC_register->CFGR0 &= (uint32_t)0xF0FF0000;
    RCC_register->CTLR &= (uint32_t)0xFEF6FFFF;
    RCC_register->CTLR &= (uint32_t)0xFFFBFFFF;
    RCC_register->CFGR0 &= (uint32_t)0xFF00FFFF;
    RCC_register->CTLR &= (uint32_t)0xEBFFFFFF;
    RCC_register->INTR = 0x00FF0000;
    RCC_register->CFGR2 = 0x00000000;
    System_clockInit();
    System_delayInit();
}

bool System::setClockChannelStatus(ClockChannel channel,bool status){
    int channel_index = (int)channel;
    int bus = channel_index / 32;
    int bus_bit = channel_index % 32;
    if ( status ){
        switch ( bus ){
            case (int)System::ClockBus::AHB : /* AHB. */
                RCC_register->AHBPCENR |= (0x01 << bus_bit);
                break;
            case (int)System::ClockBus::APB1 : /* APB1. */
                RCC_register->APB1PCENR |= (0x01 << bus_bit);
                break;
            case (int)System::ClockBus::APB2 : /* APB2. */
                RCC_register->APB2PCENR |= (0x01 << bus_bit);
                break;
        }
    }else{
        switch ( bus ){
            case (int)System::ClockBus::AHB : /* AHB. */
                RCC_register->AHBPCENR &= ~(0x01 << bus_bit);
                break;
            case (int)System::ClockBus::APB1 : /* APB1. */
                RCC_register->APB1PCENR &= ~(0x01 << bus_bit);
                break;
            case (int)System::ClockBus::APB2 : /* APB2. */
                RCC_register->APB2PCENR &= ~(0x01 << bus_bit);
                break;
        }
    }
    if ( System::getClockChannelStatus(channel) != status ){
        return false;
    }else{
        return true;
    }
}

bool System::getClockChannelStatus(ClockChannel channel){
    int channel_index = (int)channel;
    int bus = channel_index / 32;
    int bus_bit = channel_index % 32;
    uint32_t temp = 0x00000000;
    switch ( bus ){
        case (int)System::ClockBus::AHB : /* AHB. */
            temp = RCC_register->AHBPCENR & (0x01 << bus_bit);
            break;
        case (int)System::ClockBus::APB1 : /* APB1. */
            temp = RCC_register->APB1PCENR & (0x01 << bus_bit);
            break;
        case (int)System::ClockBus::APB2 : /* APB2. */
            temp = RCC_register->APB2PCENR & (0x01 << bus_bit);
            break;
    }
    if ( temp != 0x00000000 ){
        return true;
    }else{
        return false;
    }
}

unsigned int System::getClockChannelFrequency(ClockChannel channel){
    int channel_index = (int)channel;
    int bus = channel_index / 32;
    if ( bus >= 3 || bus < 0 ) return 0;
    return __system_clock_bus_frequency[bus];
}

bool System::setInterruptChannelStatus(InterruptChannel channel,bool status){
    return System::setInterruptChannelStatus((int)channel,status);
}

bool System::setInterruptChannelStatus(int channel,bool status){
    unsigned int reg_index = channel / 32;
    unsigned int reg_bit = channel % 32;
    if ( reg_index >= (sizeof(PFIC_register->IENR) / sizeof(uint32_t)) ) return false;
    if ( status ) PFIC_register->IENR[reg_index] = (0x01 << reg_bit);
    else PFIC_register->IRER[reg_index] = (0x01 << reg_bit);
    if ( System::getInterruptChannelStatus(channel) != status ){
        return false;
    }else{
        return true;
    }
}

bool System::getInterruptChannelStatus(InterruptChannel channel){
    return System::getInterruptChannelStatus((int)channel);
}

bool System::getInterruptChannelStatus(int channel){
    unsigned int reg_index = channel / 32;
    unsigned int reg_bit = channel % 32;
    uint32_t temp = 0x00000000;
    if ( reg_index >= (sizeof(PFIC_register->ISR) / sizeof(uint32_t)) ) return false;
    temp = (PFIC_register->ISR[reg_index] & (0x01 << reg_bit));
    if ( temp != 0 ){
        return true;
    }else{
        return false;
    }
}

void System::delayMs(unsigned int nms){
    long unsigned int threshold_value = system_tick + nms;
    while ( system_tick < threshold_value );
}

extern "C" __attribute__((interrupt()))
void SysTick_Handler(void){
    if ( SysTick_register->SR == 1 ){
        SysTick_register->SR = 0;
        system_tick++;
    }
}
